#include "modules/map/hdmap/adapter/odr_parser/convert_curve_offset.h"

#include "modules/map/hdmap/adapter/odr_parser/util_odr_parser.h"

#include <cmath>

namespace apollo {
namespace hdmap {
namespace adapter {

void ConvertCurveOffset::FixCurveOffset(LaneInternal& lane_internal)
{	
	PbCurve* center_curve = lane_internal.lane.mutable_central_curve();
	PbLaneBoundary* right_boundary = lane_internal.lane.mutable_right_boundary();
	PbCurve* right_curve = right_boundary->mutable_curve();
	PbLaneBoundary* left_boundary = lane_internal.lane.mutable_left_boundary();
	PbCurve* left_curve = left_boundary->mutable_curve();

    FixCurve(center_curve, true);
    FixCurve(right_curve, false);
    FixCurve(left_curve, false);
}

void ConvertCurveOffset::FixCurve(PbCurve* curve, bool is_center_curve)
{
  double s = 0;
  int seg_num = curve->segment_size();
  for (int i = 0; i < seg_num; i++) {
      PbCurveSegment* curve_segment = curve->mutable_segment(i);
      PbCurveSegment* next_curve_segment = NULL;
      if (i < seg_num - 1) {
          next_curve_segment = curve->mutable_segment(i + 1);
      }
      curve_segment->set_s(s);

      double seg_length = 0.0;
      PbLineSegment* line_segment = curve_segment->mutable_line_segment();
      int point_num = line_segment->point_size();
      for (int j = 1; j < point_num; j++) {
          const PbPoint3D& start = line_segment->point(j - 1);
          const PbPoint3D& end = line_segment->point(j);

          seg_length += PointDistance(start, end);
      }

      // get the first point of next curve segment
      if (next_curve_segment && is_center_curve) {
          const PbLineSegment& next_line_segment = next_curve_segment->line_segment();
          if (next_line_segment.point_size() > 0) {
              const PbPoint3D& next_first_point = next_line_segment.point(0);
              const PbPoint3D& pre_last_point = line_segment->point(point_num - 1);

              double distance = PointDistance(pre_last_point, next_first_point);
              if (distance > 0.01) {
                  printf("Warning: gap between curve segment is large: %f\n", distance);
              }              
              const PbPoint3D& pre_last_second_point = line_segment->point(point_num - 2);
              double distance2 = PointDistance(pre_last_second_point, next_first_point);
              // TODO: FIXME for left lane??
              if (distance2 < distance) {
                  seg_length += distance;
                  if (distance > UtilOdrParser::CurveResolution) {
                     // the gap is too large, insert some points
                     double ratio = distance / UtilOdrParser::CurveResolution;
                     for (int i = 0; i < (int)ratio; i++) {
                         double new_x = UtilOdrParser::Interpolate(pre_last_point.x(), next_first_point.x(), i / ratio);
                         double new_y = UtilOdrParser::Interpolate(pre_last_point.y(), next_first_point.y(), i / ratio);

                         // add point
                         PbPoint3D* pt = line_segment->add_point();
                         pt->set_x(new_x);
                         pt->set_y(new_y);
                         pt->set_z(0.0);
                     }
                  }
              } // else the next segment is overlapped with current segment
              else {
                  printf("Warning: overlap curve segment found.\n");
              }
          }
      }

      curve_segment->set_length(seg_length);
      s += seg_length;
  }
}

double ConvertCurveOffset::PointDistance(const PbPoint3D& start, const PbPoint3D& end)
{
  const double dx = end.x() - start.x();
  const double dy = end.y() - start.y();
  return hypot(dx, dy);
}

}
}
}